#pragma config(Sensor, in1,    pot,                 sensorPotentiometer)
#pragma config(Sensor, dgtl1,  lshaft,              sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  ultra,               sensorSONAR_inch)
#pragma config(Sensor, dgtl11, rshaft,              sensorQuadEncoder)
#pragma config(Motor,  port1,           leftArm1,      tmotorNormal, openLoop)
#pragma config(Motor,  port3,           leftArm2,      tmotorNormal, openLoop)
#pragma config(Motor,  port4,           intake1,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           leftDBMotor,   tmotorNormal, openLoop)
#pragma config(Motor,  port6,           rightDBMotor,  tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           intake2,       tmotorNormal, openLoop)
#pragma config(Motor,  port8,           rightArm1,     tmotorNormal, openLoop)
#pragma config(Motor,  port10,          rightArm2,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//DO NOT MODIFY ABOVE!!! (Configuration preprocessor directives)

/*
 * Team D VEX main and driver control competition code.
 * @author Derek Leung, Art Kalb 294 652 823 1862
 */

//Required code - do not modify!!
#pragma platform(VEX)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"

//SET THIS BEFORE ROUND IN COMPETITION

//constants
const int ZONE = 0; //0 for isolation, 1 for interaction
const int COLOR = 0; //0 for red, 1 for blue
int routine = ZONE;
int alliance = COLOR;

#define MY_BALLS_ARE_ON_FIRE 0

#include "autonomous.c"

//DEBUG
int joystickLeft1;
int joystickRight1;
int joystickLeft2;
int joystickRight2;
int dbMotorLeft;
int dbMotorRight;
int potentiometer;
int leftShaftEncoder;
int rightShaftEncoder;
int ultrasonicSensor;

//Drive-base motor values
const int THRESHOLD = 25;       //minimum motor configuration to overcome static friction
const int SPLIT_POINT = 80;     //cannot equal 0!
const int DISCRETE_MAPPINGS = 4;
const int BOTTOM_SAFE_L = -10;
const int TOP_SAFE_L = 10;
const int BOTTOM_SAFE_R = -10;
const int TOP_SAFE_R = 10;
const int ARM_SAFE=10;

//Helper functions

/**
 * Utility function for determining sign.
 *
 * @param num the number whose sign to determine.
 * @return -1 if negative, 1 if positive, 0 otherwise.
 */

/**
 * Applies a piecewise discrete-continuous function to determine motor output given
 *
 * @author Derek Leung
 *
 * @param joystick the value of the joystick from -127 to 127
 * joystick inputs.
 * @param threshold the minimum motor configuration to overcome static friction
 * @param splitPoint the point at which the continuous function becomes discrete
 * @param discreteMappings the number of partitions of which to apply the discrete
 * function
 * @param downSafe a lower "safe" bound for the input value
 * @param upSafe an upper "safe" bound for the input value. Input between this and downSafe
 * will yield a motor output of 0.
 * @return the value of which to set the motor
 */
int motorValue(int joystick, int threshold, int splitPoint, int discreteMappings, int downSafe, int upSafe)
{
  float partition, result;

  float CONTROL_INTERVAL = (127 - splitPoint) / discreteMappings;
  float MOTOR_INTERVAL = (127 - splitPoint) / (discreteMappings - 1);
  int MOTOR_RANGE = splitPoint - threshold;

  if(joystick > downSafe && joystick < upSafe)
  {
    return 0;
  }

  //Continuous mapping from 0 at threshold to splitPoint at splitPoint
  else if(joystick < splitPoint && joystick > -splitPoint)
  {
    //TODO hacked to work!! - abs(downSafe) not bi-directionally compatible!
    return sign(joystick) * (threshold + MOTOR_RANGE * ((abs(joystick) - abs(downSafe)) / (splitPoint * 1.0))); // * sign(joystick);
  } else  //Discrete mappings: 64 to 80, 80 to 96, 96 to 112, 112 to 128
  {
    result = splitPoint * sign(joystick);
    for(partition = splitPoint + 2 * CONTROL_INTERVAL; partition < 127; partition += CONTROL_INTERVAL)
    {
      if(joystick < partition && joystick > -partition)
      {
        result += MOTOR_INTERVAL * sign(joystick);
      }
    }
    return (int) result;
  }
}

//Driver control function - autonomous function is located in "autonomous.c"

/**
 * Code called during user control phase.
 *
 * This code functions as an interface between the robot driving inputs
 * and the motors.
 */
task usercontrol()
{
  int lstick, rstick; //joystick 1
  int lstick2, rstick2; //joystick 2
  /*while(1==1)
  {*/

  bool start=false;
  use_ultra=false;

  //auto_move(1000);

  StartTask(arm_control);

  while(true)
  {
    //DEBUG - in
    joystickLeft1 = vexRT[Ch3];
    joystickRight1 = vexRT[Ch2];
    joystickLeft2 = vexRT[Ch3Xmtr2];
    joystickRight2 = vexRT[Ch2Xmtr2];
    potentiometer = SensorValue(pot);
    leftShaftEncoder = SensorValue(lShaft);
    rightShaftEncoder = SensorValue(rShaft);
    ultrasonicSensor = SensorValue(ultra);
    //Variable updates
    //Joystick mappings
    lstick = vexRT[Ch3] * -1; //flip left drive base motor orientations
    rstick = vexRT[Ch2] * -1;
    lstick2 = vexRT[Ch3Xmtr2] * -1; //flip left lift motor orientations
    rstick2 = vexRT[Ch2Xmtr2];
    //Piecewise 4-step input mapping for drive base (driver joysticks)
    motor[port5] = motorValue(lstick, THRESHOLD, SPLIT_POINT, DISCRETE_MAPPINGS, BOTTOM_SAFE_L, TOP_SAFE_L);
    motor[port6] = motorValue(rstick, THRESHOLD, SPLIT_POINT, DISCRETE_MAPPINGS, BOTTOM_SAFE_R, TOP_SAFE_R);
    //Arm
    motor[port1] = lstick2 * -1;
    motor[port3] = lstick2;
    motor[port8] = lstick2 * -1;
    motor[port10] = lstick2; //kill arm if significant input
    //Intake
//      motor[port4] = rstick2 * -1;
    motor[port7] = rstick2; //intake
    //TODO Find constants and tune
    }
  }
